#ifndef __FOOT_FUNCTIONS_H__
#define __FOOT_FUNCTIONS_H__

#include "main.h"

#define L1            0.113f
#define L2            0.23f
#define L3            0.23f
#define L4            0.113f
#define L5            0.108f
#define _Degree_toRad 0.01745329251994329f
#define _Rad_toDegree 57.29577951308f
// offset 203.24f 23.24f 156.76f 23.24f
#define _2_PI         6.28318530717958647692528f
#define _Freq         0.001f
#define Wheel_R       0.104f

typedef struct
{
  /* data */
  float L0;
  float phi0;
  float phi1;
  float phi2;
  float phi2_D;
  float phi3;
  float phi3_D;
  float phi4;
  float alpha;
  float theta;
  float theta_360;
  float last_theta;
  float delta_theta;
  float phi;
  float delta_phi;
} FOOT;

typedef struct
{
  /* data */
  float pitch;
  float yaw;
  float roll;
  float gyro[3];
  float r_w_delta_angle;
  float r_w_last_angle;
  float r_w_real_angle;
  float r_w_speed;
  float l_w_delta_angle;
  float l_w_last_angle;
  float l_w_real_angle;
  float l_w_speed;
  float x_b;
  float speed;  // delta x_b
} Robot;

extern FOOT right_foot;
extern FOOT left_foot;
extern Robot robot;

void Five_Bar_Forward_kinematic(FOOT *foot, float phi4, float phi1);
void State_Data_Cal(FOOT *foot, FOOT *foot_l, Robot *robot);
void Robot_Get(Robot *robot, float roll, float yaw, float pitch, float gyro_0, float gyro_1, float gyro_2, float r_w_angle, float l_w_angle, float r_w_speed, float l_w_speed);

#endif
